//
// Created by root on 22-11-2.
//

#ifndef VIN_TEST_ESTIMATOR_H
#define VIN_TEST_ESTIMATOR_H
#include<vector>
#include<queue>
#include<mutex>
#include <pthread.h>
#include<condition_variable>
#include <eigen3/Eigen/Dense>
#include<sensor_msgs/PointCloud.h>
#include <sensor_msgs/Imu.h>

#include "util/IntegrationBase.h"

using namespace std;

class Estimator {
public:
    Estimator();
    void process();
    void processIMU(const sensor_msgs::ImuConstPtr &imu_msg);
    void processFeature(const sensor_msgs::PointCloudConstPtr &feature_msg);
    vector<pair<vector<sensor_msgs::ImuConstPtr>,sensor_msgs::PointCloudConstPtr>> getMeasurement();
    void IMU_Preintegration(double t, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity);
    void clearImuAndFeature();
    void processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header);

    enum SolverFlag
    {
        INITIAL,
        NON_LINEAR
    };

    enum MarginalizationFlag
    {
        MARGIN_OLD = 0,
        MARGIN_SECOND_NEW = 1
    };
    SolverFlag solver_flag;
    MarginalizationFlag  marginalization_flag;

    IntegrationBase *pre_integrations[11];
    IntegrationBase *tmp_pre_integration;
    Vector3d g;
    MatrixXd Ap[2], backup_A;
    VectorXd bp[2], backup_b;

    Matrix3d ric[1];
    Vector3d tic[1];

    Vector3d Ps[11];
    Vector3d Vs[11];
    Matrix3d Rs[11];
    Vector3d Bas[11];
    Vector3d Bgs[11];
    double td;
    vector<double> dt_buf[11];
    vector<Vector3d> linear_acceleration_buf[11];
    vector<Vector3d> angular_velocity_buf[11];

    int frame_count;
    Vector3d acc_0, gyr_0;
    bool first_imu=false;
    double current_time = -1;
    queue<sensor_msgs::ImuConstPtr> imu_buf;
    queue<sensor_msgs::PointCloudConstPtr> fea_buf;
    mutex m_sign;
    mutex m_estimator;
    condition_variable con;
};


#endif //VIN_TEST_ESTIMATOR_H
